#include <vector>
#include <iostream>
using std::vector;

#include "edge.h"
#include "graph.h"
#include "library/array_queue.h"
#include "library/hash_table.h"
#include "library/triplet.h"
#include "limits.h"
#include "library/binary_heap.h"

using namespace std;

// Helper function to backtrace the path given the bookkeeping for
// BFS or Dijkstra in the form of a hash table
template<typename V, typename E, typename W>
vector<V> getPath(HashTable< V, Triplet<bool, int, V>* >  * table, V src,
    V dest, Graph<V,E,W>* g);
// Helper function to get the bookkeeping for BFS, since this is used by
// other functions as well
template <typename V, typename E, typename W>
HashTable<V, Triplet<bool, int, V>* > *
    getBFSTable(V src, V dest, Graph<V, E, W> * g);

// Do BFS
template <typename V, typename E, typename W>
vector<V> bfs(V src, V dest, Graph<V,E,W>* g) {
  HashTable<V, Triplet<bool, int, V>* >  * table = 
    getBFSTable(src, dest, g);
  return getPath(table, src, dest, g);
}

// Get the path given the hash table book keeping
template <typename V, typename E, typename W>
vector<V> getPath(HashTable< V, Triplet<bool, int, V>* >  * table, V src,
    V dest, Graph<V, E, W> * g) {
  // Check if end vertex was accessible
  if (table->get(dest)->first == false) {
    cout << "\nError: Path Not Found to Destination Vertex.\n";
    return vector<V>();
  }

  // Backtrack until the start vertex is found
  vector<V> toReturn;
  V from = dest;
  while (from != src) {
    toReturn.push_back(from); 
    from = table->get(from)->third;
  }
  toReturn.push_back(src);

  // Clean up dynamically allocated memory
  vector< Triplet<bool, int, V> * > toDelete = 
    table->getValues();
  for (int i=0; i<toDelete.size(); i++) {
    delete toDelete[i];
  }
  delete table;
  return toReturn;
}

// Carry out BFS and return the bookkeeping table that results
template <typename V, typename E, typename W>
HashTable<V, Triplet<bool, int, V>* > *
getBFSTable(V src, V dest, Graph<V, E, W> * g) {
 
  // Get vertices
  Queue<V> * queue = new ArrayQueue<V>();
  HashTable<V, Triplet<bool, int, V>* >  * table = 
    new HashTable<V, Triplet<bool, int, V> * > (53, 0.8) ;
  
  // Initialize bookkeeping table
  vector<V> vertices = g->getAllVertices();
  for (int i=0; i<vertices.size(); i++) {
    table->insert(vertices[i], 
        new Triplet<bool, int, V>(false, INT_MAX, V()));
  }

  table->get(src)->second = 0;
  V cur = src;
  queue->enqueue(cur);
  // While we haven't found the dest vertex yet, visit all neighbors
  // Stop if there are no more accessible vertices to visit
  while (! queue->isEmpty() and cur != dest) {
    cur = queue->dequeue();
    table->get(cur)->first = true;
    vector<V> neighbors = g->getNeighbors(cur);
    int wt = table->get(cur)->second + 1;
    for (int i=0; i<neighbors.size(); i++){
      if (table->get(neighbors[i])->first == false) {
        table->get(neighbors[i])->first = true;
        table->get(neighbors[i])->second = wt;
        table->get(neighbors[i])->third = cur;
        queue->enqueue(neighbors[i]);
      }
    }
  }
  delete queue;
  return table;
}


// Carry out Dijkstra's algorithm
// Implementation is similar to BFS, but use a pq to find the least
// cost vertex at any given stage
template <typename V, typename E, typename W>
vector<V> dijkstra(V src, V dest, Graph<V,E,W>* g) {

  BinaryHeap<int, V> * pq = new BinaryHeap<int, V>();
  
  HashTable<V, Triplet<bool, int, V>* >  * table = 
    new HashTable<V, Triplet<bool, int, V> * > (53, 0.8) ;
  
  // Initialize table
  vector<V> vertices = g->getAllVertices();
  for (int i=0; i<vertices.size(); i++) {
    table->insert(vertices[i], 
        new Triplet<bool, int, V>(false, INT_MAX, V()));
    pq->insert(INT_MAX, vertices[i]);
  }

  table->get(src)->second = 0;
  pq->update(0, src);
  V cur = src;

  // While we haven't found the dest vertex yet and there are still
  // vertices to visit
  while (! pq->isEmpty() and cur != dest) {
    // Visit least costly edge
    cur = pq->removeMin();
    table->get(cur)->first = true;
    // Compute weight of all neighbors that are not yet visited
    vector<V> neighbors = g->getNeighbors(cur);
    int dist = table->get(cur)->second;
    for (int i=0; i<neighbors.size(); i++){
      if (table->get(neighbors[i])->first == false) {
        int cur_wt = table->get(neighbors[i])->second;
        int alt_wt; 
        alt_wt = dist + g->getMinEdge(cur, neighbors[i]).getWeight();
        if (cur_wt > alt_wt) {
          table->get(neighbors[i])->second = alt_wt;
          table->get(neighbors[i])->third = cur;
          pq->update(alt_wt, neighbors[i]);
        }
      }
    }
  }

  delete pq;
 
  return getPath(table, src, dest, g);
}



template <typename V, typename E, typename W>
int getUnweightedPathLength(V src, V dest, Graph<V,E,W>* g) {
  return (bfs(src, dest, g).size()-1);
}

// Get the weighted path length by doing dijstra and then summing
// across the path
template <typename V, typename E, typename W>
int getWeightedPathLength(V src, V dest, Graph<V,E,W>* g) {
  vector<V> path = dijkstra(src, dest, g);
  int length = 0;
  for (int i=1; i<path.size(); i++) {
    
    length += g->getMinEdge(path[i], path[i-1]).getWeight();
  }

  return length;
}

// Find the average distance from src to all other nodes accessible
// in the graph (with BFS)
template <typename V, typename E, typename W>
double averageDistance(V src, Graph<V,E,W>* g) {

  HashTable<V, Triplet<bool, int, V>* >  * table = 
    getBFSTable(src, V(), g);

  int count = 0;
  double total_dist = 0;

  vector< Triplet<bool, int, V> * > all_vecs = table->getValues();
  for (int i=0; i<all_vecs.size(); i++) {
    if (all_vecs[i]->first) {
      count ++;
      total_dist += all_vecs[i]->second;
    }
  }

  // Decrement count by 1 so we don't count src vertex twice
  count --;

  vector< Triplet<bool, int, V> * > toDelete = 
    table->getValues();
  for (int i=0; i<toDelete.size(); i++) {
    delete toDelete[i];
  }


  delete table;
  return total_dist / count;

 
}
